#include <stdio.h>
#include <unistd.h>
#include <wiringPi.h>
#include <vector>
#include <iostream>

#include "photosensitive_camera.h"
#include "steering_engine.h"
#include "step_motor.h"
#include "tcp_send.h"
#include "ultrasonic_ranging.h"
#include "waste_sorting.h"


void setStrSend(int type,float usage,float* usage_array,int& recyleBitMap,std::string& strSend);

int main(int argc,char* argv[]){
    photosensitive_camera::PhotoSentiveCamera* psc0=new photosensitive_camera::PhotoSentiveCamera(21);
    steering_engine::SteerEngine* se0=new steering_engine::SteerEngine(26);
    step_motor::StepMotor* sm0=new step_motor::StepMotor(22,23,24,25,8);
    ultrasonic_ranging::UltraSonicRanging* usr0=new ultrasonic_ranging::UltraSonicRanging(4,5);
    waste_sorting::WasteSorting* ws0=new waste_sorting::WasteSorting();

    std::string strReturn="";
    std::string strSend="{\"id\":6,\"recyleBitMap\":14,\"location\":[120.3532,30.3242],\"usage\":[0.9, 1, 1, 0.5]}";
    std::vector<std::string>* objectsNames = new std::vector<std::string>();
    int type = 0;
    float usage_array[4] = {0,0,0,0};
    int recyleBitMap=15;
    float usage=0;

    //一些调试用的代码
    if (argc == 3) {
        //可以微调步进电机走指定的步数，参数格式如下：./main 0(1) 128
        int oritation=atoi(argv[1]);
        int steps = atoi(argv[2]);
        switch(oritation){
            case(0):{
                sm0->RotateClockwise(steps);
                break;
            }
            case(1):{
                sm0->RotateAntiClockwise(steps);
                break;
            }
            default:
                break;
        }
        goto exit;
    }
    
    //主体逻辑
    do{
        //等待引脚电平
        psc0->WaitForDO(nullptr,nullptr);
        //拍照
        psc0->TakeAPic("500");
        //识别照片中物体类型
        //ws0->getGarbageNamesByImage("garbage.jpg",objectsNames);
        objectsNames->push_back(argv[1]);

        /*
           0-可回收
           1-有害
           2-厨余(湿)
           3-其他(干)
        */
        //判断所属垃圾类别
        type=ws0->getGarbageCategoryByNames(objectsNames);
        //步进电机转过对应角度
        sm0->RotateClockwise(type<<7);
        sm0->Stop();

        //舵机投放垃圾
        se0->RotateByAngle(90);
        delayMicroseconds(700*1000);
        se0->RotateByAngle(0);

        //步进电机转回对应角度
        sm0->RotateAntiClockwise(type<<7);
        sm0->Stop();

#if 0   
        float dis=0;
        //测距
        dis = usr0->DisMeasure();
        //超声波测距模块距离桶底的高度为22cm，距离桶顶的高度为8.7cm
        usage = (22 - dis) / 13;
        //保留一位小数
        usage=static_cast<double>(static_cast<int>(usage*10+0.5))/10;
#endif

        //生成待发送的字符串
        setStrSend(type,usage,usage_array,recyleBitMap,strSend);
        //建立TCP连接，将数据发送过去
        tcp_send::TCPClient* tcpclient0=new tcp_send::TCPClient("192.168.0.5",9526);
        std::cout<<__LINE__<<std::endl;
        tcpclient0->SendStrViaTCP(strSend,&strReturn);
        //assert(strReturn=="OK");
        delete(tcpclient0);
        tcpclient0=nullptr;
    }while(0);

exit:    
    delete(psc0);
    delete(se0);
    delete(sm0);
    delete(usr0);
    delete(ws0);
    return 0;
}

void setStrSend(int type,float usage,float* usage_array,int& recyleBitMap,std::string& strSend){
    usage_array[type]=usage;
    if(usage>0.9){
        recyleBitMap|=(1<<type);
    }
    //strSend="{\"id\":0,\"recyleBitMap\":14,\"location\":[120.3532,30.3242],\"usage\":[0.9, 1, 1, 0.5]}";
    strSend="{\"id\":2,\"recyleBitMap\":"+std::to_string(recyleBitMap)+",\"location\":[120.332,30.342],\"usage\":[";
    strSend+=std::to_string(usage_array[0]);
    for(int i=1;i<4;++i){
        strSend+=","+std::to_string(usage_array[i]);
    }
    strSend+="]}";
    std::cout<<strSend<<std::endl;
    return;
}